#ifndef NAVIGATION_MANAGER_HPP
#define NAVIGATION_MANAGER_HPP

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "wearhouse_navigation/action/navigate_to_pose.hpp" // 已定义的action
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <vector>
#include <memory>
#include <unordered_map>

namespace wearhouse_navigation {


    struct Waypoint {
        double x;
        double y;
        double theta;
        std::string description; // 区分起始点、中间点和终点
    };


    class NavigationManager : public rclcpp::Node
    {
    public:
        explicit NavigationManager();

    private:
        using NavigateToPose = wearhouse_navigation::action::NavigateToPose;
        using GoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;

        rclcpp_action::Client<NavigateToPose>::SharedPtr action_client_;
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr target_well_sub_;

        // 存储井编号到路径（包含中间点和最终点）的映射
        std::unordered_map<std::string, std::vector<Waypoint>> well_path_map_;

        std::vector<Waypoint> current_path_;
        size_t current_waypoint_index_;
        bool is_executing_plan_;

        void loadWellPathMap(); //从参数服务器加载映射
        void targetWellCallback(const std_msgs::msg::String::SharedPtr msg);
        void startNavigationPlan(const std::string& well_id);
        void sendNextWaypoint();
        void goalResponseCallback(std::shared_future<GoalHandle::SharedPtr> future);
        void feedbackCallback(GoalHandle::SharedPtr goal_handle,
            const std::shared_ptr<const NavigateToPose::Feedback> feedback);
        void resultCallback(const GoalHandle::WrappedResult & result);

    };

} // namespace wearhouse_navigation

#endif // NAVIGATION_PLANNER_HPP